#include <ros/ros.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>

extern "C"{

// #include “OOXX.h”
#include "Serial.h"

}

#include <iostream>
#include "ros/ros.h"
#include "std_msgs/Int32MultiArray.h"
#include "std_msgs/Float32MultiArray.h"
#include <iomanip>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <pthread.h>
#include <fstream>
#include "pod_io.h"
#include <cstdlib>
#include <string>
#include <ctime>
#include <time.h>
#include "serial_io.hpp"
using namespace std;

#define CRC32_POLYNOMIAL 0xEDB88320L

int Counter = 0;


bool  data_updated = false;
std::string m_port_id;
std::string m_odom_topic_name;


/* --------------------------------------------------------------------------
Calculate a CRC value to be used by CRC calculation functions.
-------------------------------------------------------------------------- */
unsigned long CRC32Value(int i)
{
int j;
unsigned int ulCRC;
ulCRC = i;
for ( j = 8 ; j > 0; j-- )
{
if ( ulCRC & 1 )
ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
else
ulCRC >>= 1;
}
return ulCRC;
}
/* --------------------------------------------------------------------------
Calculates the CRC-32 of a block of data all at once
-------------------------------------------------------------------------- */
unsigned long CalculateBlockCRC32( unsigned long ulCount, Uint32 *ucBuffer ) {
	unsigned long ulTemp1;
	unsigned long ulTemp2;
	unsigned long ulCRC = 0;
	while ( ulCount-- != 0 ) {
		ulTemp1 = ( ulCRC >> 8 ) & 0x00FFFFFFL;
		ulTemp2 = CRC32Value( ((int) ulCRC ^ *ucBuffer++ ) & 0xff );
		ulCRC = ulTemp1 ^ ulTemp2;
	}
	return ulCRC ;
}


double x, y, z, qx, qy, qz, qw, vx, vy, vz, lx, ly, lz;
int pod_fd;

// static Message m;
Message m; 


// static int m.FrameCnt = 0;

// define TX_BUF_LEN 1024
unsigned char sent_buf[TX_BUF_LEN];

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{ 
 data_updated = true;
   // m
  std::time_t now = std::time(0);
  std::tm* local_time = std::localtime(&now);

  m.MessageLength = 132;
  // m.MessageID = 0x07e5;

  m.Year = local_time->tm_year + 1900-208;
  m.Month = local_time->tm_mon + 1;
  m.Day = local_time->tm_mday;
  m.Hour = local_time->tm_hour;
  m.Minute = local_time->tm_min;
  m.Second = local_time->tm_sec;
  // m.Milesecond
  
  m.lx = msg->twist.twist.angular.x;
  m.ly = msg->twist.twist.angular.y;
  m.lz = msg->twist.twist.angular.z;
  m.x = msg->pose.pose.position.x;
  m.y = msg->pose.pose.position.y;
  m.z = msg->pose.pose.position.z;
  m.vx = msg->twist.twist.linear.x;
  m.vy = msg->twist.twist.linear.y;
  m.vz = msg->twist.twist.linear.z;

  m.deltaX = 1.0;
  m.deltaY = 1.0;
  m.deltaZ = 1.0;
  m.HAcc = 1.0;
  m.VAcc = 1.0;

  m.DeviceNumber = 123456;
  m.role = 1;
  m.GNSS_status = 2;
  m.UWB_status = 1;
  m.FrameCnt = Counter + 1;
  m.satelliteNumber = 18;
  Counter++;
  // set to 0
  m.SpeedGround = 0;
  m.Cog = 0;
  m.latitude_f = msg->twist.twist.angular.x;
  m.longitude_f = msg->twist.twist.angular.y;
  m.altitude_f = msg->twist.twist.angular.z;
  m.velocityX_f = msg->twist.twist.linear.x;
  m.velocityY_f = msg->twist.twist.linear.y;
  m.velocityZ_f = msg->twist.twist.linear.z;

}

int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)	//set uart param��and the main function
{
	struct termios newtio, oldtio;         //see what it can be done if delete.Generally to save old configuration when close.
	if(tcgetattr(fd, &oldtio) != 0) { 
		perror("com get err");
		return -1;
	}

	bzero(&newtio, sizeof(newtio));
	newtio.c_cflag  |=  CLOCAL | CREAD;
	newtio.c_cflag &= ~CSIZE;

	switch(nBits)
	{
	case 7:
		newtio.c_cflag |= CS7;
		break;
	case 8:
		newtio.c_cflag |= CS8;
		break;
	}

	switch(nEvent)
	{
	case 'O':
		newtio.c_cflag |= PARENB;
		newtio.c_cflag |= PARODD;
		newtio.c_iflag |= (INPCK | ISTRIP);
		break;
	case 'E': 
		newtio.c_iflag |= (INPCK | ISTRIP);
		newtio.c_cflag |= PARENB;
		newtio.c_cflag &= ~PARODD;
		break;
	case 'N':  
		newtio.c_cflag &= ~PARENB;
		break;
	}

	switch(nSpeed)
	{
	case 2400:
		cfsetispeed(&newtio, B2400);
		cfsetospeed(&newtio, B2400);
		break;
	case 4800:
		cfsetispeed(&newtio, B4800);
		cfsetospeed(&newtio, B4800);
		break;
	case 9600:
		cfsetispeed(&newtio, B9600);
		cfsetospeed(&newtio, B9600);
		break;
	case 38400:
		cfsetispeed(&newtio, B38400);
		cfsetospeed(&newtio, B38400);
		break;
	case 115200:
		cfsetispeed(&newtio, B115200);
		cfsetospeed(&newtio, B115200);
		break;
	case 460800:
		cfsetispeed(&newtio, B460800);
		cfsetospeed(&newtio, B460800);
		break;
	default:
		cfsetispeed(&newtio, B9600);
		cfsetospeed(&newtio, B9600);
		break;
	}
	if( nStop == 1 )
		newtio.c_cflag &=  ~CSTOPB;
	else if ( nStop == 2 )
	newtio.c_cflag |=  CSTOPB;
	newtio.c_cc[VTIME]  = 0;
	newtio.c_cc[VMIN] = 0;
	tcflush(fd, TCIFLUSH);
	if((tcsetattr(fd, TCSANOW, &newtio)) != 0)
	{
		perror("com set error");
		return -1;
	}
    //printf("set done!\n\r");
	return 0;
}


void open_uart(void){		//open uart in ttyUSB0
// UART 是一种串行通信协议

	char pod_mode;
	//const char *eot_uart = "/dev/ttyUSB0";
	char dir = '0';

	//string eot_uart("/dev/ttyUSB0");
  
	// get_serial_io("FTDI_USB_Serial_Converter",port_id);
	string eot_uart(m_port_id);  // 在/dev/ttyUSB1上打开一个连接，以便与某个外部设备进行通信(该外部设备是飞控，本体是机载计算机)
    //eot_uart += dir;
	//printf(".........................");
	cout << "----------------" << eot_uart << "----------------" << endl;
	std::cout << m_port_id << std::endl;

	if((pod_fd = open(eot_uart.c_str(), O_RDWR|O_NOCTTY))<0)
		cout << "open " << eot_uart << " is failed" << endl;

	set_opt(pod_fd, 115200, 8, 'N', 1); // 将连接参数设置为460800波特率，8个数据位，无奇偶校验和1个停止位

	memset(sent_buf, 0, strlen((char *)sent_buf)); // 将名为sent_buf的缓冲区初始化为全零
}


void pod_sent_cmd(servo_mode mode, double x, double y, double z, double vx, double vy, double vz, double lx, double ly, double lz) 
// void pod_sent_cmd(double x, double y, double z, double qx, double qy, double qz, double qw, double vx, double vy, double vz, double lx, double ly, double lz) 
// 该函数的作用是将括号里的n个参数打包成一个数据帧，并通过串口发送出去。
{
	// short int set_yaw, set_pitch;
	int sent_nbyte;

	// set_yaw = yaw_rate * 100;
	// set_pitch = pitcsh_rate * 100;

	// memset(sent_buf, 0, strlen((char *)sent_buf));
	
	// sent_nbyte = write(pod_fd, sent_buf, 13);
  	sent_nbyte = write(pod_fd, sent_buf, strlen((char *)sent_buf));
	// 通过write函数将数据帧发送出去

    /*	
	for(int i = 0;i < 34;i++)
	{
		printf("i :%d  data:  %02x \n",i,sent_buf[i]);
	}*/
}
// servo_mode mode = SERVO_ZERO; // mode值需要确定，此处仍待定


int main(int argc, char** argv)
{
  ros::init(argc, argv, "odom_subscriber_node"); // 设置节点（node）名字
  ros::NodeHandle nh; // 用于管理节点资源，调用api

  ros::Rate rate(100);//10hz
  nh.param<std::string>("xy_port_id", m_port_id,"/dev/ttyCH343USB0");
  std::cout << m_port_id << std::endl;
  nh.param<std::string>("odom_topic_name", m_odom_topic_name,"/suav/filter/odom");

  ros::Subscriber odom_sub = nh.subscribe(m_odom_topic_name, 10, odomCallback);


  //ros::Rate rate(100);
   open_uart();
  // sent_buf[0]=0x01;
   //			int  bytesWritten = write(pod_fd, sent_buf, strlen((char *)sent_buf));
    unsigned long send_crc;//CRC最终结果
    unsigned int CRC_count = 160;//CRC缓存遍历使用
    //结构体实例化
    U32_Byte U32_DATA;
    i32_Byte i32_DATA;
    i16_Byte i16_DATA;
    U16_Byte U16_DATA;
    U64_Byte U64_DATA;
    doub_Byte doub_DATA;
    sing_Byte sing_DATA;
    Uint32 buf32_crc[160];


   while(nh.ok()){

   ros::spinOnce();
   if(data_updated == false)
   		continue;
   	data_updated = false;
  // m.lx = 39.6530308541;
	// m.ly = 116.098554979;
	// m.lz = 115;
	  printf("m.lx == %0.8f      m.ly== %lf      m.lz== %lf \r\n",m.lx,m.ly,m.lz);
	  printf("m.latitude_f== %0.8f   m.longitude_f== %0.8f   m.altitude_f== %lf \r\n",m.latitude_f,m.longitude_f,m.altitude_f);

    
  	int sent_nbyte;
	
    sent_buf[0] = 0xAA;  //Sync
    sent_buf[1] = 0x44;  //Sync
    sent_buf[2] = 0x12;  //Sync
    sent_buf[3] = 0x1C;  //Header length
	
    // U16_DATA.U16 = (Uint16)(m.MessageID);  //Message ID
    sent_buf[4] = 0xe5; // U16_DATA.BYTE.Byte_0;
    sent_buf[5] = 0x07; //U16_DATA.BYTE.Byte_1;
    
    sent_buf[6] = 0x00;  //Message type
    sent_buf[7] = 0x00;  //Reserved no.7

    U16_DATA.U16 = 132; // (Uint16)(m.MessageLength);  //Message len (no header and crc)
    // 20+24+13*4+24+12=132
    sent_buf[8] = U16_DATA.BYTE.Byte_0;
    sent_buf[9] = U16_DATA.BYTE.Byte_1;
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    sent_buf[10] = U16_DATA.BYTE.Byte_0;
    sent_buf[11] = U16_DATA.BYTE.Byte_1;
    sent_buf[12] = 0x00;  //Reserved
    sent_buf[13] = 0x00;  //Reserved
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    sent_buf[14] = U16_DATA.BYTE.Byte_0;
    sent_buf[15] = U16_DATA.BYTE.Byte_1;
    U32_DATA.U32 = (Uint32)(0);  //Reserved
    sent_buf[16] = U32_DATA.BYTE.Byte_0;
    sent_buf[17] = U32_DATA.BYTE.Byte_1;
    sent_buf[18] = U32_DATA.BYTE.Byte_2;
    sent_buf[19] = U32_DATA.BYTE.Byte_3;
    U32_DATA.U32 = (Uint32)(0);  //Reserved
    sent_buf[20] = U32_DATA.BYTE.Byte_0;
    sent_buf[21] = U32_DATA.BYTE.Byte_1;
    sent_buf[22] = U32_DATA.BYTE.Byte_2;
    sent_buf[23] = U32_DATA.BYTE.Byte_3;
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    sent_buf[24] = U16_DATA.BYTE.Byte_0;
    sent_buf[25] = U16_DATA.BYTE.Byte_1;
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    sent_buf[26] = U16_DATA.BYTE.Byte_0;
    sent_buf[27] = U16_DATA.BYTE.Byte_1;

    // 帧内容
    U64_DATA.U64 = (Uint64)(m.DeviceNumber);  //Device number
    sent_buf[28] = U64_DATA.BYTE.Byte_0;
    sent_buf[29] = U64_DATA.BYTE.Byte_1;
    sent_buf[30] = U64_DATA.BYTE.Byte_2;
    sent_buf[31] = U64_DATA.BYTE.Byte_3;
    sent_buf[32] = U64_DATA.BYTE.Byte_4;
    sent_buf[33] = U64_DATA.BYTE.Byte_5;

    sent_buf[34] = (Uint8)(m.role); //设备角色，0：锚节点，1：侦察节点
    sent_buf[35] = (Uint8)(m.Year); //UTC-年
    sent_buf[36] = (Uint8)(m.Month); //UTC-月
    sent_buf[37] = (Uint8)(m.Day); //UTC-日
    sent_buf[38] = (Uint8)(m.Hour); //UTC-时
    sent_buf[39] = (Uint8)(m.Minute); //UTC-分
    sent_buf[40] = (Uint8)(m.Second); //UTC-秒
    U16_DATA.U16 = (Uint16)(m.Second/1000);  //UTC-毫秒
    sent_buf[41] = U16_DATA.BYTE.Byte_0;
    sent_buf[42] = U16_DATA.BYTE.Byte_1;

    U16_DATA.U16 = (Uint16)(m.FrameCnt);  //帧计数
    sent_buf[43] = U16_DATA.BYTE.Byte_0;
    sent_buf[44] = U16_DATA.BYTE.Byte_1;

    sent_buf[45] = (Uint8)(m.GNSS_status);  //0：未定位，1：单点，2：伪距差分，3：RTKINT，4RTKfloat
    sent_buf[46] = (Uint8)(m.UWB_status);  //
  
    doub_DATA.doub = (double)(m.lx);    //纬度，单位：度
    sent_buf[47] = doub_DATA.BYTE.Byte_0;
    sent_buf[48] = doub_DATA.BYTE.Byte_1;
    sent_buf[49] = doub_DATA.BYTE.Byte_2;
    sent_buf[50] = doub_DATA.BYTE.Byte_3;
    sent_buf[51] = doub_DATA.BYTE.Byte_4;
    sent_buf[52] = doub_DATA.BYTE.Byte_5;
    sent_buf[53] = doub_DATA.BYTE.Byte_6;
    sent_buf[54] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.ly);    //经度，单位：度
    sent_buf[55] = doub_DATA.BYTE.Byte_0;
    sent_buf[56] = doub_DATA.BYTE.Byte_1;
    sent_buf[57] = doub_DATA.BYTE.Byte_2;
    sent_buf[58] = doub_DATA.BYTE.Byte_3;
    sent_buf[59] = doub_DATA.BYTE.Byte_4;
    sent_buf[60] = doub_DATA.BYTE.Byte_5;
    sent_buf[61] = doub_DATA.BYTE.Byte_6;
    sent_buf[62] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.lz);    //海拔高度，单位：米
    sent_buf[63] = doub_DATA.BYTE.Byte_0;
    sent_buf[64] = doub_DATA.BYTE.Byte_1;
    sent_buf[65] = doub_DATA.BYTE.Byte_2;
    sent_buf[66] = doub_DATA.BYTE.Byte_3;
    sent_buf[67] = doub_DATA.BYTE.Byte_4;
    sent_buf[68] = doub_DATA.BYTE.Byte_5;
    sent_buf[69] = doub_DATA.BYTE.Byte_6;
    sent_buf[70] = doub_DATA.BYTE.Byte_7;
    sing_DATA.sing = (float)(m.HAcc); //卫星定位水平精度因子 HDOP 值  16
    sent_buf[71] = sing_DATA.BYTE.Byte_0;
    sent_buf[72] = sing_DATA.BYTE.Byte_1;
    sent_buf[73] = sing_DATA.BYTE.Byte_2;
    sent_buf[74] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.HAcc); //卫星定位水平精度因子 VDOP 值   17
    sent_buf[75] = sing_DATA.BYTE.Byte_0;
    sent_buf[76] = sing_DATA.BYTE.Byte_1;
    sent_buf[77] = sing_DATA.BYTE.Byte_2;
    sent_buf[78] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.x); //相对位置 X 轴坐标，m  18
    sent_buf[79] = sing_DATA.BYTE.Byte_0;
    sent_buf[80] = sing_DATA.BYTE.Byte_1;
    sent_buf[81] = sing_DATA.BYTE.Byte_2;
    sent_buf[82] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.y); //相对位置 Y 轴坐标，m  19
    sent_buf[83] = sing_DATA.BYTE.Byte_0;
    sent_buf[84] = sing_DATA.BYTE.Byte_1;
    sent_buf[85] = sing_DATA.BYTE.Byte_2;
    sent_buf[86] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.z); //相对位置 Z 轴坐标，m  20
    sent_buf[87] = sing_DATA.BYTE.Byte_0;
    sent_buf[88] = sing_DATA.BYTE.Byte_1;
    sent_buf[89] = sing_DATA.BYTE.Byte_2;
    sent_buf[90] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.deltaX); //UWB x 误差，m  21
    sent_buf[91] = sing_DATA.BYTE.Byte_0;
    sent_buf[92] = sing_DATA.BYTE.Byte_1;
    sent_buf[93] = sing_DATA.BYTE.Byte_2;
    sent_buf[94] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.deltaY); //UWB y 误差，m  22
    sent_buf[95] = sing_DATA.BYTE.Byte_0;
    sent_buf[96] = sing_DATA.BYTE.Byte_1;
    sent_buf[97] = sing_DATA.BYTE.Byte_2;
    sent_buf[98] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.deltaZ); //UWB z 误差，m  23
    sent_buf[99] = sing_DATA.BYTE.Byte_0;
    sent_buf[100] = sing_DATA.BYTE.Byte_1;
    sent_buf[101] = sing_DATA.BYTE.Byte_2;
    sent_buf[102] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.vx); //X 轴速度，m/s   24
    sent_buf[103] = sing_DATA.BYTE.Byte_0;
    sent_buf[104] = sing_DATA.BYTE.Byte_1;
    sent_buf[105] = sing_DATA.BYTE.Byte_2;
    sent_buf[106] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.vy); //Y 轴速度，m/s   25
    sent_buf[107] = sing_DATA.BYTE.Byte_0;
    sent_buf[108] = sing_DATA.BYTE.Byte_1;
    sent_buf[109] = sing_DATA.BYTE.Byte_2;
    sent_buf[110] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.vz); //Z 轴速度，m/s   26
    sent_buf[111] = sing_DATA.BYTE.Byte_0;
    sent_buf[112] = sing_DATA.BYTE.Byte_1;
    sent_buf[113] = sing_DATA.BYTE.Byte_2;
    sent_buf[114] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.SpeedGround); //卫星地面速度，单位：节  27
    sent_buf[115] = sing_DATA.BYTE.Byte_0;
    sent_buf[116] = sing_DATA.BYTE.Byte_1;
    sent_buf[117] = sing_DATA.BYTE.Byte_2;
    sent_buf[118] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.Cog); //卫星地面航向，北起顺时针，单位：度  28
    sent_buf[119] = sing_DATA.BYTE.Byte_0;
    sent_buf[120] = sing_DATA.BYTE.Byte_1;
    sent_buf[121] = sing_DATA.BYTE.Byte_2;
    sent_buf[122] = sing_DATA.BYTE.Byte_3;
    doub_DATA.doub = (m.latitude_f);    //室内外融合后纬度，单位：度  29
    sent_buf[123] = doub_DATA.BYTE.Byte_0;
    sent_buf[124] = doub_DATA.BYTE.Byte_1;
    sent_buf[125] = doub_DATA.BYTE.Byte_2;
    sent_buf[126] = doub_DATA.BYTE.Byte_3;
    sent_buf[127] = doub_DATA.BYTE.Byte_4;
    sent_buf[128] = doub_DATA.BYTE.Byte_5;
    sent_buf[129] = doub_DATA.BYTE.Byte_6;
    sent_buf[130] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.longitude_f);    //室内外融合后经度，单位：度  30
    sent_buf[131] = doub_DATA.BYTE.Byte_0;
    sent_buf[132] = doub_DATA.BYTE.Byte_1;
    sent_buf[133] = doub_DATA.BYTE.Byte_2;
    sent_buf[134] = doub_DATA.BYTE.Byte_3;
    sent_buf[135] = doub_DATA.BYTE.Byte_4;
    sent_buf[136] = doub_DATA.BYTE.Byte_5;
    sent_buf[137] = doub_DATA.BYTE.Byte_6;
    sent_buf[138] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.altitude_f);    //室内外融合后海拔高度，单位：米  31
    sent_buf[139] = doub_DATA.BYTE.Byte_0;
    sent_buf[140] = doub_DATA.BYTE.Byte_1;
    sent_buf[141] = doub_DATA.BYTE.Byte_2;
    sent_buf[142] = doub_DATA.BYTE.Byte_3;
    sent_buf[143] = doub_DATA.BYTE.Byte_4;
    sent_buf[144] = doub_DATA.BYTE.Byte_5;
    sent_buf[145] = doub_DATA.BYTE.Byte_6;
    sent_buf[146] = doub_DATA.BYTE.Byte_7;
    sing_DATA.sing = (float)(m.velocityX_f); //室内外融合后X 轴速度，m/s  32
    sent_buf[147] = sing_DATA.BYTE.Byte_0;
    sent_buf[148] = sing_DATA.BYTE.Byte_1;
    sent_buf[149] = sing_DATA.BYTE.Byte_2;
    sent_buf[150] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.velocityY_f); //室内外融合后Y 轴速度，m/s  33
    sent_buf[151] = sing_DATA.BYTE.Byte_0;
    sent_buf[152] = sing_DATA.BYTE.Byte_1;
    sent_buf[153] = sing_DATA.BYTE.Byte_2;
    sent_buf[154] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.velocityZ_f); //室内外融合后Z 轴速度，m/s   34
    sent_buf[155] = sing_DATA.BYTE.Byte_0;
    sent_buf[156] = sing_DATA.BYTE.Byte_1;
    sent_buf[157] = sing_DATA.BYTE.Byte_2;
    sent_buf[158] = sing_DATA.BYTE.Byte_3;

    sent_buf[159] = (Uint8)(m.satelliteNumber);    //卫星数量   35

    // U32_DATA.U32 = (Uint32)(0); //Reserved
    // sent_buf[160] = U32_DATA.BYTE.Byte_0;
    // sent_buf[161] = U32_DATA.BYTE.Byte_1;
    // sent_buf[162] = U32_DATA.BYTE.Byte_2;
    // sent_buf[163] = U32_DATA.BYTE.Byte_3;

    // CRC校验和

    for(int i = 0; i < 160; i++)
    {
        buf32_crc[i] = sent_buf[i];
    }

    send_crc = CalculateBlockCRC32(CRC_count, buf32_crc);
    U32_DATA.U32 = (Uint32)send_crc; //crc
    sent_buf[160] = U32_DATA.BYTE.Byte_0;
    sent_buf[161] = U32_DATA.BYTE.Byte_1;
    sent_buf[162] = U32_DATA.BYTE.Byte_2;
    sent_buf[163] = U32_DATA.BYTE.Byte_3;


    for(int i=0;i<164;i++){
		printf(",0x%x ",sent_buf[i]);
   

	}
	  printf("\r\n");


    sent_nbyte = write(pod_fd, sent_buf, 164);
    send_crc =0;
    memset(sent_buf, 0, strlen((char *)sent_buf)); // 将名为sent_buf的缓冲区初始化为全零

    rate.sleep();
   }
    //ros::spin();
    close(pod_fd);


  return 0;
}

